#include "navigation.h"
#include "behavior.h"
#include "math.h"
#include "kuruma.h"

int state = 0;
float distance = 0;
float direction = 0;
int state1 = 0;
float sub_distance = 0;

int navigate(Position *pos) {
    static int state = 0;
    static float distance = 0;
    static float start_x = 0.0f, start_y = 0.0f;  // 缓起步用

    float dx = pos->x - odom.x;
    float dy = pos->y - odom.y;
    distance = sqrtf(dx*dx + dy*dy);

    // === 阶段 0: 先旋转到目标直线方向 ===
    if (state == 0) {
        float target_dir = wrap_angle(atan2f(-dx, dy));
        if (turn_to_angle(target_dir * RAD_TO_DEG)) {
            state = 1; // 转向完成
            start_x = odom.x;   // 记录起步位置
            start_y = odom.y;
        }
        return 0;
    }

    // === 阶段 1: 边走边修正 ===
    if (state == 1) {
        if (distance < 0.03f) { // 3cm 内认为到达
            kuruma_set_velocity(0, 0);
            state = 2;
            return 0;
        }

        float target_dir = wrap_angle(atan2f(-dx, dy));
        float err = wrap_angle(target_dir - odom.theta);

        float k_ang = 2.0f; // 转向增益

        // --- 缓起步速度计算 ---
        float max_linear = fminf(distance, 0.5f); // 距离越近速度越低，上限 0.5m/s
        float ramp_dist = 0.3f;                   // 缓起步距离：前 0.3 m 内渐增
        float traveled = sqrtf((odom.x - start_x)*(odom.x - start_x) +
                               (odom.y - start_y)*(odom.y - start_y));
        float ramp_factor = fminf(traveled / ramp_dist, 1.0f);

        float linear = fmaxf(0.1f, max_linear * ramp_factor);
        float angular = k_ang * err;

        kuruma_set_velocity(linear, angular);
        return 0;
    }

    // === 阶段 2: 最终转向 ===
    if (state == 2) {
        if (turn_to_angle(pos->yaw)) {
            state = 0;
            return 1; // 完成
        }
    }

    return 0;
}


//int navigate(Position *pos) { //直线导航方式
//	
//	if(distance == 0) { //计算距离
//		float dx = pos->x-odom.x;
//		float dy = pos->y-odom.y;
//		distance = sqrt(dx*dx+dy*dy);
//		if(fabsf(dx) <= 0.005) {
//			direction = 0;
//		} else {
//			direction = wrap_angle(atan2f(-dx,dy));
//		}
//	}
//	//第一步，先旋转到指定角度
//	if(state == 0) {
//		state += turn_to_angle(direction*RAD_TO_DEG);
//	}
//	//第二步，修正轨迹与前进
//	if(state == 1) {
//		if(distance <= 0.1) { //若目的地在10cm之内，直接前进
//			state += drive_straight(distance,0.1);
//		} else { //若目的地大于10cm, 考虑修正轨迹
//			if(state1 == 0) { //先走5cm
//				state1 += drive_straight(0.05,0.1); 
//			}
//			if(state1 == 1) { //再修正轨迹
//				state1 += drive_arc_by_angle_pid(0.2,(direction-odom.theta)*RAD_TO_DEG,30);
//			}
//			if(state1 == 2) { //再走到目的地
//				float dx = pos->x-odom.x;
//				float dy = pos->y-odom.y;
//				sub_distance = sqrt(dx*dx+dy*dy);
//				if(drive_straight(sub_distance,fminf(sub_distance/3.0f,0.3f))) {
//					state1 = 0;
//					sub_distance = 0;
//					state += 1;
//				}
//			}
//		}
//	}
//	//第三步，到达目的地后，调整方向
//	if(state == 2) {
//		if(turn_to_angle(pos->yaw)) {
//			state = 0;
//			distance = 0;
//			sub_distance = 0;
//			return 1;
//		}
//	}
//	return 0;
//}
